#include <QLoggingCategory>
#include <QMessageBox>
#include "rockyassisterx.h"
#include <QCloseEvent>
#include <QSettings>

rockyAssisterX::rockyAssisterX(QWidget *parent)
	: QMainWindow(parent)
{
	m_serial = NULL;
	m_ser_neure = NULL;
	m_r = m_g = m_b = false;
	m_send_timer = new QTimer();

	/*Setup UI*/
	ui.setupUi(this);
	ui.gridLayout->removeItem(ui.horizontalSpacer);
	m_comSwBtn = new SwitchControl(ui.groupBox);
	ui.gridLayout->addWidget(m_comSwBtn, 0, 0, 1, 2);
	m_serialOpen = m_comSwBtn->isToggled();
	updateComPortList();

	QRegExp regExp("[0-9]{0,10}");
	ui.rgbPollLineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.colorPolllineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.visibleEnvPolllineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.visibleGrayPolllineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.visibleReflectPolllineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.irReflectPolllineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.ObstaclePollLineEdit->setValidator(new QRegExpValidator(regExp, this));
	ui.timinglineEdit->setValidator(new QRegExpValidator(regExp, this));

	QRegExp sendRegExp("[0-9,a-f,A-F ]*");
	ui.SendlineEdit->setValidator(new QRegExpValidator(sendRegExp, this));
	
	QRegExp pidIntRegExp("[0-9]*");
	ui.lineEdit_speed->setValidator(new QRegExpValidator(pidIntRegExp, this));
	ui.lineEdit_max_incream->setValidator(new QRegExpValidator(pidIntRegExp, this));
	ui.lineEdit_cycle->setValidator(new QRegExpValidator(pidIntRegExp, this));
	ui.lineEdit_trace_grace->setValidator(new QRegExpValidator(pidIntRegExp, this));
	
	QRegExp pidRegExp("[0-9]*[.]{1}[0-9]*");
	ui.lineEdit_Kp->setValidator(new QRegExpValidator(pidRegExp, this));
	ui.lineEdit_Ki->setValidator(new QRegExpValidator(pidRegExp, this));
	ui.lineEdit_Kd->setValidator(new QRegExpValidator(pidRegExp, this));
	ui.lineEdit_Kd_lowpass->setValidator(new QRegExpValidator(pidRegExp, this));

	ui_enable(false);

	/*App setting*/
	readSettings();

	/*Signal & slots*/
	connect(m_comSwBtn, &SwitchControl::toggled, this, &rockyAssisterX::comPortToggle);
	connect(this, &rockyAssisterX::comPortUpdateSw, this->m_comSwBtn, &SwitchControl::setToggle);
}

rockyAssisterX::~rockyAssisterX()
{

}

void rockyAssisterX::comPortToggle(bool checked)
{
	if (checked)
	{
		comPortOpen();
	}
	else
	{
		comPortClose();
	}
}

void rockyAssisterX::comPortOpen(void)
{
	if (m_serial)
	{
		m_serial->close();
		delete(m_serial);
	}

	m_serial = new QSerialPort;
	m_serial->setPortName(ui.comPortBox->currentText());
	if (!m_serial->setBaudRate(ui.baudrateComboBox->currentText().toInt()))
	{
		QMessageBox::warning(this, tr("Open error"), QString::fromLatin1("set baud rate encounter an error:") + QString::number(m_serial->error()));
		return;
	}
	if (!m_serial->open(QIODevice::ReadWrite))
	{
		QMessageBox::warning(this, tr("Open error"), QString::fromLatin1("open encounter an error:") + QString::number(m_serial->error()));
		return;
	}

	m_serial->setDataBits(QSerialPort::Data8);
	m_serial->setParity(QSerialPort::NoParity);
	m_serial->setStopBits(QSerialPort::OneStop);
	m_serial->setFlowControl(QSerialPort::NoFlowControl);
	m_serialOpen = true;

	// UI update
	emit comPortUpdateSw(m_serialOpen);

	// neure
	m_ser_neure = new neure(m_serial);
	connect(m_ser_neure, &neure::logInfo, this, &rockyAssisterX::logOut);
	connect(m_ser_neure, &neure::devOnlineUpdate, this, &rockyAssisterX::devOnlineUpdate);
	connect(m_ser_neure, &neure::firmwareUpdate, this, &rockyAssisterX::firmwareUpdate);
	connect(m_ser_neure, &neure::rgbUpdate, this, &rockyAssisterX::rgbUpdate);
	connect(m_ser_neure, &neure::colorUpdate, this, &rockyAssisterX::colorUpdate);
	connect(m_ser_neure, &neure::envUpdate, this, &rockyAssisterX::envUpdate);
	connect(m_ser_neure, &neure::grayUpdate, this, &rockyAssisterX::grayUpdate);
	connect(m_ser_neure, &neure::visibleReflectUpdate, this, &rockyAssisterX::visibleReflectUpdate);
	connect(m_ser_neure, &neure::irReflectUpdate, this, &rockyAssisterX::irReflectUpdate);
	connect(m_ser_neure, &neure::obstacleUpdate, this, &rockyAssisterX::obstacleUpdate);
	connect(m_ser_neure, &neure::dcCurrentUpdate, this, &rockyAssisterX::dcCurrentUpdate);

	ui_enable(true);
}

void rockyAssisterX::comPortClose(void)
{
	if (m_serial)
	{
		m_pid_timer.stop();
		m_serialOpen = false;

		delete(m_ser_neure);
		emit comPortUpdateSw(m_serialOpen);
		
		m_serial->waitForBytesWritten(1000);
		m_serial->close();
		delete(m_serial);

		m_serial = NULL;
		m_ser_neure = NULL;
	}

	ui_enable(false);
}

void rockyAssisterX::updateComPortList(void)
{
	ui.comPortBox->clear();
	const QList<QSerialPortInfo> infos = QSerialPortInfo::availablePorts();
	for (const QSerialPortInfo &info : infos)
	{
		ui.comPortBox->addItem(info.portName());
	}
}

void rockyAssisterX::on_comPortBox_clicked(void)
{
}

void rockyAssisterX::logOut(QString log)
{
	ui.logTextEdit->moveCursor(QTextCursor::End);
	ui.logTextEdit->insertPlainText(log);
}

void rockyAssisterX::devOnlineUpdate(bool onOff, quint8 devId)
{
	if (onOff)
	{
		ui.onlineProgressBar->setValue(100);
		ui.devIdLineEdit->setText(QString(char(devId + '0')));
	}
	else
	{
		ui.onlineProgressBar->setValue(0);
		ui.devIdLineEdit->clear();
	}
}

void rockyAssisterX::firmwareUpdate(QString frimware)
{
	ui.firmwareLineEdit->setText(frimware);
}

void rockyAssisterX::rgbUpdate(quint16 r, quint16 g, quint16 b)
{
	QString rgbText;

	r = r & 0xFF;
	g = g & 0xFF;
	b = b & 0xFF;
	rgbText = "#" + QString::number(r, 16) + QString::number(g, 16) + QString::number(b, 16);
	ui.rgbLineEdit->setText(rgbText);
	ui.rgbDisLabel->setRgb(r, g, b);
}

void rockyAssisterX::colorUpdate(quint16 r, quint16 g, quint16 b, QString color_id)
{
	r = r & 0xFF;
	g = g & 0xFF;
	b = b & 0xFF;
	ui.colorLineEdit->setText(color_id);
	ui.colorDisLabel->setRgb(r, g, b);
}

void rockyAssisterX::envUpdate(quint8 env_gray)
{
	ui.visibleEnvLineEdit->setText(QString::number(env_gray));
}

void rockyAssisterX::grayUpdate(quint8 gray)
{
	ui.visibleWhiteReflectLineEdit->setText(QString::number(gray));
}

void rockyAssisterX::visibleReflectUpdate(quint8 visible_reflect)
{
	ui.visibleReflectLineEdit->setText(QString::number(visible_reflect));
}

void rockyAssisterX::irReflectUpdate(quint8 ir_reflect)
{
	ui.irReflectLineEdit->setText(QString::number(ir_reflect));
}

void rockyAssisterX::obstacleUpdate(quint8 obstacle)
{
	ui.ifObstacleLineEdit->setText(QString::number(obstacle));
}

void rockyAssisterX::dcCurrentUpdate(quint16 l, quint16 r)
{
	ui.leftDcSampleCurrentLineEdit->setText(QString::number(l));
	ui.rightDcSampleCurrentLineEdit->setText(QString::number(r));
}

void rockyAssisterX::serial_read_data(void)
{
	QByteArray buf;
	buf = m_serial->readAll();
	ui.logTextEdit->setText(QString::fromLocal8Bit(buf));
	
	if (!buf.isEmpty())
	{
		QString recv_str = QString::fromLocal8Bit(buf);
	}
	buf.clear();
}

void rockyAssisterX::ui_enable(bool flag)
{
	ui.pushButton_Disc->setEnabled(flag);
	ui.stopPushButton->setEnabled(flag);
	ui.leftTurnPushButton->setEnabled(flag);
	ui.rightTurnPushButton->setEnabled(flag);
	ui.leftDcMotorHorizontalSlider->setEnabled(flag);
	ui.rightDcMotorHorizontalSlider->setEnabled(flag);
	ui.forwardHorizontalSlider->setEnabled(flag);
	ui.backwardHorizontalSlider->setEnabled(flag);
	ui.caliPushButton->setEnabled(flag);
	ui.getRgbPushButton->setEnabled(flag);
	ui.getColorPushButton->setEnabled(flag);
	ui.getVisibleEnvPushButton->setEnabled(flag);
	ui.getVisibleWhiteReflectPushButton->setEnabled(flag);
	ui.getVisibleReflectPushButton->setEnabled(flag);
	ui.getIrReflectPushButton->setEnabled(flag);
	ui.ifObstaclePushButton->setEnabled(flag);
	ui.LeftDcMotorSpinBox->setEnabled(flag);
	ui.rightDcMotorSpinBox->setEnabled(flag);
	ui.forwardSpinBox->setEnabled(flag);
	ui.backwardSpinBox->setEnabled(flag);
	ui.setRpushButton->setEnabled(flag);
	ui.setGpushButton->setEnabled(flag);
	ui.setBpushButton->setEnabled(flag);

	// Polling
	ui.rgbPollCheckBox->setEnabled(flag);
	ui.colorPollCheckBox->setEnabled(flag);
	ui.VisibleEnvPollCheckBox->setEnabled(flag);
	ui.VisibleGrayPollCheckBox->setEnabled(flag);
	ui.visibleReflectPollCheckBox->setEnabled(flag);
	ui.irReflectPollCheckBox->setEnabled(flag);
	ui.ObstaclePollCheckBox->setEnabled(flag);
	if (!flag)
	{
		ui.rgbChangeRadioButton->setEnabled(flag);
		ui.colorChangeRadioButton->setEnabled(flag);
		ui.visibleEnvChangeRadioButton->setEnabled(flag);
		ui.visibleGrayChangeRadioButton->setEnabled(flag);
		ui.visibleReflectChangeRadioButton->setEnabled(flag);
		ui.irReflectChangeRadioButton->setEnabled(flag);
		ui.obstacleChangeRadioButton->setEnabled(flag);

		ui.rgbPeriodRadioButton->setEnabled(flag);
		ui.colorPeriodRadioButton->setEnabled(flag);
		ui.visibleEnvPeriodRadioButton->setEnabled(flag);
		ui.visibleGrayPeriodRadioButton->setEnabled(flag);
		ui.visibleReflectPeriodRadioButton->setEnabled(flag);
		ui.irReflectPeriodRadioButton->setEnabled(flag);
		ui.obstaclePeriodRadioButton->setEnabled(flag);

		ui.rgbPollLineEdit->setEnabled(flag);
		ui.colorPolllineEdit->setEnabled(flag);
		ui.visibleEnvPolllineEdit->setEnabled(flag);
		ui.visibleGrayPolllineEdit->setEnabled(flag);
		ui.visibleReflectPolllineEdit->setEnabled(flag);
		ui.irReflectPolllineEdit->setEnabled(flag);
		ui.ObstaclePollLineEdit->setEnabled(flag);
	}
	else
	{
		emit ui.rgbPollCheckBox->toggled(ui.rgbPollCheckBox->checkState());
		emit ui.colorPollCheckBox->toggled(ui.colorPollCheckBox->checkState());
		emit ui.VisibleEnvPollCheckBox->toggled(ui.VisibleEnvPollCheckBox->checkState());
		emit ui.VisibleGrayPollCheckBox->toggled(ui.VisibleGrayPollCheckBox->checkState());
		emit ui.visibleReflectPollCheckBox->toggled(ui.visibleReflectPollCheckBox->checkState());
		emit ui.irReflectPollCheckBox->toggled(ui.irReflectPollCheckBox->checkState());
		emit ui.ObstaclePollCheckBox->toggled(ui.ObstaclePollCheckBox->checkState());
	}

	// Console
	ui.SendlineEdit->setEnabled(flag);
	ui.SendPushButton->setEnabled(flag);
	ui.HexCheckBox->setEnabled(flag);
	ui.TimingSendcheckBox->setEnabled(flag);
	ui.ClearPushButton->setEnabled(flag);
	ui.timinglineEdit->setEnabled(flag);

	// Pid
	ui.lineEdit_Kp->setEnabled(flag);
	ui.lineEdit_Ki->setEnabled(flag);
	ui.lineEdit_Kd->setEnabled(flag);
	ui.pushButton_start->setEnabled(flag);
	ui.pushButton_stop->setEnabled(flag);
	ui.lineEdit_cycle->setEnabled(flag);
	ui.lineEdit_speed->setEnabled(flag);
	ui.lineEdit_max_incream->setEnabled(flag);
	ui.lineEdit_trace_grace->setEnabled(flag);
	ui.lineEdit_Kd_lowpass->setEnabled(flag);
}

void rockyAssisterX::on_comPortBox_currentIndexChanged(const QString &text)
{
}

void rockyAssisterX::on_comPortBox_highlighted(const QString &Text)
{
}

void rockyAssisterX::on_comPortBox_activated(const QString &Text)
{
}

void rockyAssisterX::on_pushButton_Disc_clicked(void)
{
	if (m_ser_neure)
	{
		m_ser_neure->assignDevId();
	}
}

/*
void rockyAssisterX::on_forwardHorizontalSlider_valueChanged(int value)
{
	m_ser_neure->forward(value);
}
*/

void rockyAssisterX::on_backwardHorizontalSlider_valueChanged(int value)
{
	m_ser_neure->backward(value);
}

void rockyAssisterX::on_leftDcMotorHorizontalSlider_valueChanged(int value)
{
	m_ser_neure->motorControl(ui.leftDcMotorHorizontalSlider->value(), ui.rightDcMotorHorizontalSlider->value());
}

void rockyAssisterX::on_rightDcMotorHorizontalSlider_valueChanged(int value)
{
	m_ser_neure->motorControl(ui.leftDcMotorHorizontalSlider->value(), ui.rightDcMotorHorizontalSlider->value());
}

void rockyAssisterX::on_leftTurnPushButton_clicked(void)
{
	m_ser_neure->leftTurn();
}

void rockyAssisterX::on_rightTurnPushButton_clicked(void)
{
	m_ser_neure->rightTurn();
}

void rockyAssisterX::on_stopPushButton_clicked(void)
{
	m_ser_neure->stop();
	ui.leftDcMotorHorizontalSlider->setValue(0);
	ui.rightDcMotorHorizontalSlider->setValue(0);
	ui.forwardHorizontalSlider->setValue(0);
	ui.backwardHorizontalSlider->setValue(0);
}

void rockyAssisterX::on_DcSampleCurrentCheckBox_toggled(bool sw)
{
	m_ser_neure->pollingDcCurrent(sw);
}

void rockyAssisterX::on_setRpushButton_clicked(void)
{
	m_ser_neure->setR();
	m_r = !m_r;
	if (m_r)
	{
		ui.setRpushButton->setText("R off");
	}
	else
	{
		ui.setRpushButton->setText("R on");
	}
}
void rockyAssisterX::on_setGpushButton_clicked(void)
{
	m_ser_neure->setG();
	m_g = !m_g;
	if (m_g)
	{
		ui.setGpushButton->setText("G off");
	}
	else
	{
		ui.setGpushButton->setText("G on");
	}
}
void rockyAssisterX::on_setBpushButton_clicked(void)
{
	m_ser_neure->setB();
	m_b = !m_b;
	if (m_b)
	{
		ui.setBpushButton->setText("B off");
	}
	else
	{
		ui.setBpushButton->setText("B on");
	}
}

// Color
void rockyAssisterX::on_caliPushButton_clicked(void)
{
	m_ser_neure->calibration();
}

void rockyAssisterX::on_getRgbPushButton_clicked(void)
{
	m_ser_neure->getRgb();
}

void rockyAssisterX::on_getColorPushButton_clicked(void)
{
	m_ser_neure->getColor();
}

void rockyAssisterX::on_getVisibleEnvPushButton_clicked(void)
{
	m_ser_neure->getEnv();
}

void rockyAssisterX::on_getVisibleWhiteReflectPushButton_clicked(void)
{
	m_ser_neure->getGray();
}

void rockyAssisterX::on_getVisibleReflectPushButton_clicked(void)
{
	m_ser_neure->getVisibleReflect();
}

void rockyAssisterX::on_getIrReflectPushButton_clicked(void)
{
	m_ser_neure->getIrReflect();
}

void rockyAssisterX::on_ifObstaclePushButton_clicked(void)
{
	m_ser_neure->getObstacle();
}

// Polling
// rgb
void rockyAssisterX::on_rgbPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.rgbPollLineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingRgb(checked);
	}
	else
	{
		ui.rgbPollLineEdit->setEnabled(ui.rgbPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.rgbChangeRadioButton->isChecked())
				m_ser_neure->pollingRgb();
			else
			{
				m_ser_neure->pollingRgb(ui.rgbPollLineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_rgbChangeRadioButton_toggled(bool checked)
{
	emit ui.rgbPollCheckBox->toggled(ui.rgbPollCheckBox->checkState());
}
void rockyAssisterX::on_rgbPollLineEdit_textChanged(QString str)
{
	emit ui.rgbPollCheckBox->toggled(ui.rgbPollCheckBox->checkState());
}

// color
void rockyAssisterX::on_colorPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.colorPolllineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingColor(checked);
	}
	else
	{
		ui.colorPolllineEdit->setEnabled(ui.colorPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.colorChangeRadioButton->isChecked())
				m_ser_neure->pollingColor();
			else
			{
				m_ser_neure->pollingColor(ui.colorPolllineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_colorChangeRadioButton_toggled(bool checked)
{
	emit ui.colorPollCheckBox->toggled(ui.colorPollCheckBox->checkState());
}
void rockyAssisterX::on_colorPolllineEdit_textChanged(QString str)
{
	emit ui.colorPollCheckBox->toggled(ui.colorPollCheckBox->checkState());
}

// env
void rockyAssisterX::on_VisibleEnvPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.visibleEnvPolllineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingEnv(checked);
	}
	else
	{
		ui.visibleEnvPolllineEdit->setEnabled(ui.visibleEnvPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.visibleEnvChangeRadioButton->isChecked())
				m_ser_neure->pollingEnv();
			else
			{
				m_ser_neure->pollingEnv(ui.visibleEnvPolllineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_visibleEnvChangeRadioButton_toggled(bool checked)
{
	emit ui.VisibleEnvPollCheckBox->toggled(ui.VisibleEnvPollCheckBox->checkState());
}
void rockyAssisterX::on_visibleEnvPolllineEdit_textChanged(QString str)
{
	emit ui.VisibleEnvPollCheckBox->toggled(ui.VisibleEnvPollCheckBox->checkState());
}

// gray
void rockyAssisterX::on_VisibleGrayPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.visibleGrayPolllineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingGray(checked);
	}
	else
	{
		ui.visibleGrayPolllineEdit->setEnabled(ui.visibleGrayPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.visibleGrayChangeRadioButton->isChecked())
				m_ser_neure->pollingGray();
			else
			{
				m_ser_neure->pollingGray(ui.visibleGrayPolllineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_visibleGrayChangeRadioButton_toggled(bool checked)
{
	emit ui.VisibleGrayPollCheckBox->toggled(ui.VisibleGrayPollCheckBox->checkState());
}
void rockyAssisterX::on_visibleGrayPolllineEdit_textChanged(QString str)
{
	emit ui.VisibleGrayPollCheckBox->toggled(ui.VisibleGrayPollCheckBox->checkState());
}

// visible reflect
void rockyAssisterX::on_visibleReflectPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.visibleReflectPolllineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingVisibleReflect(checked);
	}
	else
	{
		ui.visibleReflectPolllineEdit->setEnabled(ui.visibleReflectPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.visibleReflectChangeRadioButton->isChecked())
				m_ser_neure->pollingVisibleReflect();
			else
			{
				m_ser_neure->pollingVisibleReflect(ui.visibleReflectPolllineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_visibleReflectChangeRadioButton_toggled(bool checked)
{
	emit ui.visibleReflectPollCheckBox->toggled(ui.visibleReflectPollCheckBox->checkState());
}
void rockyAssisterX::on_visibleReflectPolllineEdit_textChanged(QString str)
{
	emit ui.visibleReflectPollCheckBox->toggled(ui.visibleReflectPollCheckBox->checkState());
}

// ir reflect
void rockyAssisterX::on_irReflectPollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.irReflectPolllineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingIrReflect(checked);
	}
	else
	{
		ui.irReflectPolllineEdit->setEnabled(ui.irReflectPeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.irReflectChangeRadioButton->isChecked())
				m_ser_neure->pollingIrReflect();
			else
			{
				m_ser_neure->pollingIrReflect(ui.irReflectPolllineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_irReflectChangeRadioButton_toggled(bool checked)
{
	emit ui.irReflectPollCheckBox->toggled(ui.irReflectPollCheckBox->checkState());
}
void rockyAssisterX::on_irReflectPolllineEdit_textChanged(QString str)
{
	emit ui.irReflectPollCheckBox->toggled(ui.irReflectPollCheckBox->checkState());
}

// obstacle
void rockyAssisterX::on_ObstaclePollCheckBox_toggled(bool checked)
{
	if (!checked)
	{
		ui.ObstaclePollLineEdit->setEnabled(checked);
		if (m_ser_neure)
			m_ser_neure->pollingObstacle(checked);
	}
	else
	{
		ui.ObstaclePollLineEdit->setEnabled(ui.obstaclePeriodRadioButton->isChecked());
		if (m_ser_neure)
		{
			if (ui.obstacleChangeRadioButton->isChecked())
				m_ser_neure->pollingObstacle();
			else
			{
				m_ser_neure->pollingObstacle(ui.ObstaclePollLineEdit->text().toUInt());
			}
		}
	}
}
void rockyAssisterX::on_obstacleChangeRadioButton_toggled(bool checked)
{
	emit ui.ObstaclePollCheckBox->toggled(ui.ObstaclePollCheckBox->checkState());
}
void rockyAssisterX::on_ObstaclePollLineEdit_textChanged(QString str)
{
	emit ui.ObstaclePollCheckBox->toggled(ui.ObstaclePollCheckBox->checkState());
}

// trace line
void rockyAssisterX::on_pushButton_start_clicked(void)
{
	// Pid
	m_Kp = ui.lineEdit_Kp->text().toFloat();
	m_Ki = ui.lineEdit_Ki->text().toFloat();
	m_Kd = ui.lineEdit_Kd->text().toFloat();
	m_err = m_last_err = m_last_last_err = 0.0;
	m_Kd_last_out = 0;
	m_Kd_lowpass = ui.lineEdit_Kd_lowpass->text().toFloat();

	// Trace speed setting
	m_trace_gray = ui.lineEdit_trace_grace->text().toInt();
	m_trace_speed = ui.lineEdit_speed->text().toInt();
	m_max_incream = ui.lineEdit_max_incream->text().toInt();
	m_cur_incream = 0;

	// statistics
	m_err_cnt = 0;
	m_err_sum = 0;
	avg_rsp_time_cnt = 0;
	avg_rsp_time_sum = 0;
	m_rsp_time_clear = true;

	connect(&m_pid_timer, &QTimer::timeout, this, &rockyAssisterX::pid_proc);
	connect(m_ser_neure, &neure::visibleReflectUpdate, this, &rockyAssisterX::pid_recv_feedback);
	m_pid_timer.start(ui.lineEdit_cycle->text().toInt());
	m_elapsedTimer.start();
}

void rockyAssisterX::on_pushButton_stop_clicked(void)
{
	disconnect(&m_pid_timer, &QTimer::timeout, this, &rockyAssisterX::pid_proc);
	disconnect(m_ser_neure, &neure::visibleReflectUpdate, this, &rockyAssisterX::pid_recv_feedback);
	m_pid_timer.stop();
	m_ser_neure->stop();
}

void rockyAssisterX::pid_proc(void)
{
	if (m_serialOpen)
	{
		if (m_rsp_time_clear)
		{
			m_ser_neure->getVisibleReflect();
			m_t0 = m_elapsedTimer.elapsed();
			m_rsp_time_clear = false;
		}
	}
	else
	{
		m_pid_timer.stop();
	}
}

void rockyAssisterX::pid_recv_feedback(quint8 visibleReflectGray)
{
	float p, i, d, pid_out;

	m_rsp_time_clear = true;
	m_t1 = m_elapsedTimer.elapsed();
	m_err = m_trace_gray - visibleReflectGray;
	p = m_Kp * (m_err - m_last_err);
	i = m_Ki * m_err;
	d = m_Kd * (m_err - 2*m_last_err + m_last_last_err);
	d = d * m_Kd_lowpass + (1 - m_Kd_lowpass) * m_Kd_last_out;
	m_Kd_last_out = d;
	m_last_last_err = m_last_err;
	m_last_err = m_err;

	// qDebug() << "p: " << p << "i: " << i << "d: " << d;
	pid_out = p + i + d;
	if (m_cur_incream > m_max_incream)
	{
		if (pid_out < 0)
		{
			m_cur_incream += pid_out;
		}
	}
	else if (m_cur_incream < -m_max_incream)
	{
		if (pid_out > 0)
		{
			m_cur_incream += pid_out;
		}
	}
	else
	{
		m_cur_incream += pid_out;
	}

	if (m_cur_incream > m_max_incream)
	{
		m_cur_incream = m_max_incream;
	}
	else if (m_cur_incream < -m_max_incream)
	{
		m_cur_incream = -m_max_incream;
	}

	m_ser_neure->motorControl(m_trace_speed + m_cur_incream, m_trace_speed - m_cur_incream);
	
	m_err_cnt++;
	m_err_sum += m_err;
	avg_rsp_time_sum += m_t1 - m_t0;
	avg_rsp_time_cnt++;
	ui.lineEdit_avg_err_gray->setText(QString::number(m_err_sum / m_err_cnt));
	ui.lineEdit_err->setText(QString::number(m_err));
	ui.lineEdit_left_speed->setText(QString::number(m_trace_speed + m_cur_incream));
	ui.lineEdit_right_speed->setText(QString::number(m_trace_speed - m_cur_incream));
	ui.lineEdit_respond_time->setText(QString::number(avg_rsp_time_sum / avg_rsp_time_cnt));
}

// Console
void rockyAssisterX::on_ClearPushButton_clicked(void)
{
	ui.logTextEdit->clear();
}

void rockyAssisterX::on_SendPushButton_clicked(void)
{
	QString data;

	data = ui.SendlineEdit->text();
	if (!ui.HexCheckBox->checkState())
	{
		m_ser_neure->send(data.toLatin1());
	}
	else
	{
		data.replace(" ", "");
		m_ser_neure->send(QByteArray::fromHex(data.toLatin1()));
	}
}

void rockyAssisterX::on_TimingSendcheckBox_toggled(bool sw)
{
	quint32 timing;
	if (sw)
	{
		timing = ui.timinglineEdit->text().toUInt();
		if (0 != timing)
		{
			m_send_timer->start(ui.timinglineEdit->text().toUInt());
			connect(m_send_timer, &QTimer::timeout, this, &rockyAssisterX::on_SendPushButton_clicked);
		}
		else
		{
			ui.TimingSendcheckBox->setChecked(false);
		}
	}
	else
	{
		m_send_timer->stop();
	}
}

void rockyAssisterX::on_timinglineEdit_textChanged(QString str)
{
	emit ui.TimingSendcheckBox->toggled(ui.TimingSendcheckBox->checkState());
}

// system event
void rockyAssisterX::closeEvent(QCloseEvent *event)
{
	writeSettings();
	comPortClose();
	event->accept();
}

void rockyAssisterX::writeSettings(void)
{
	QSettings settings;
	settings.beginGroup("comPortGroup");
	settings.setValue("comBaudrate", ui.baudrateComboBox->currentIndex());
	settings.setValue("comPort", ui.comPortBox->currentIndex());
	settings.endGroup();

	settings.beginGroup("pid");
	settings.setValue("Kp", ui.lineEdit_Kp->text());
	settings.setValue("Ki", ui.lineEdit_Ki->text());
	settings.setValue("Kd", ui.lineEdit_Kd->text());
	settings.setValue("lowpass", ui.lineEdit_Kd_lowpass->text());
	settings.setValue("speed", ui.lineEdit_speed->text());
	settings.setValue("max_incream", ui.lineEdit_max_incream->text());
	settings.setValue("cycle", ui.lineEdit_cycle->text());
	settings.setValue("trace_gray", ui.lineEdit_trace_grace->text());
	settings.endGroup();
}

void rockyAssisterX::readSettings(void)
{
	QSettings settings;
	settings.beginGroup("comPortGroup");
	ui.baudrateComboBox->setCurrentIndex(settings.value("comBaudrate", 0).toInt());
	ui.comPortBox->setCurrentIndex(settings.value("comPort", 0).toInt());
	settings.endGroup();

	settings.beginGroup("pid");
	ui.lineEdit_Kp->setText(QString(settings.value("Kp", "1.0").toString()));
	ui.lineEdit_Ki->setText(QString(settings.value("Ki", "0.0").toString()));
	ui.lineEdit_Kd->setText(QString(settings.value("Kd", "2.5").toString()));
	ui.lineEdit_Kd_lowpass->setText(QString(settings.value("lowpass", "1.0").toString()));
	ui.lineEdit_speed->setText(QString(settings.value("speed", "30").toString()));
	ui.lineEdit_max_incream->setText(QString(settings.value("max_incream", "30").toString()));
	ui.lineEdit_cycle->setText(QString(settings.value("cycle", "20").toString()));
	ui.lineEdit_trace_grace->setText(QString(settings.value("trace_gray", "45").toString()));
	settings.endGroup();
}
